#include <json/json.h>
#include "DroneState.h"
#include <vector>

std::ostream & operator<<(std::ostream & os, const DroneState & msg)
{
	os << "DroneNumber is:" << msg.DroneNumber << std::endl;
	os << "acceleration is:(" << msg.acceleration[0] << "," << msg.acceleration[1] << "," << msg.acceleration[2] << ")" << std::endl;
	os << "position is:(" << msg.position[0] << "," << msg.position[1] << "," << msg.position[2] << ")" << std::endl;
	os << "orientation is:(" << msg.orientation[0] << "," << msg.orientation[1] << "," << msg.orientation[2] << "," << msg.orientation[3] << ")" << std::endl;
	os << "velocity is:(" << msg.velocity[0] << "," << msg.velocity[1] << "," << msg.velocity[2] << ")" << std::endl;
	os << "mode is: " << msg.mode << std::endl;
	os << "bridgeMessage is: " << msg.bridgeMessage << std::endl;
	return os;
}


DroneState::DroneState()
{
	DroneNumber = 0;
	acceleration = std::vector<double>(3, 0);
	position = std::vector<double>(3, 0);
	orientation = std::vector<double>(4, 0);
	velocity = std::vector<double>(3, 0);
	mode = "";
	bridgeMessage = "";
}


DroneState::DroneState(int drone_number)
{
	DroneNumber = drone_number;
	acceleration = std::vector<double>(3, 0);
	position = std::vector<double>(3, 0);
	orientation = std::vector<double>(4, 0);
	velocity = std::vector<double>(3, 0);
	mode = "";
	bridgeMessage = "";
}

void DroneState::clear()
{
	acceleration = std::vector<double>(3, 0);
	position = std::vector<double>(3, 0);
	orientation = std::vector<double>(4, 0);
	velocity = std::vector<double>(3, 0);
	bridgeMessage = "";
}

std::string DroneState::droneState2Json()
{
	Json::Value data;
	data["droneNumber"] = DroneNumber;
	data["positionX"] = position[0];
	data["positionY"] = position[1];
	data["positionZ"] = position[2];
	data["orientationX"] = orientation[0];
	data["orientationY"] = orientation[1];
	data["orientationZ"] = orientation[2];
	data["orientationW"] = orientation[3];
	data["velocityX"] = velocity[0];
	data["velocityY"] = velocity[1];
	data["velocityZ"] = velocity[2];
	data["mode"] = mode;
	data["bridgeMessage"] = bridgeMessage;
	Json::FastWriter fw;
	return fw.write(data);
}

void DroneState::getMsg(const std::string& recvMsg, std::vector<DroneState>& swarmsMsg)
{
	Json::Reader reader;
	Json::Value recv;
	if (reader.parse(recvMsg.c_str(), recv))
	{
		int swarmNumbers = recv["swarmSize"].asInt();
		Json::Value vec = recv["drones"];
		swarmsMsg.resize(swarmNumbers);
		for (int i = 0; i < swarmNumbers; ++i) {
			Json::Value data = vec[i];
			swarmsMsg[i].DroneNumber = data["droneNumber"].asInt();
			swarmsMsg[i].position[0] = data["positionX"].asDouble();
			swarmsMsg[i].position[1] = data["positionY"].asDouble();
			swarmsMsg[i].position[2] = data["positionZ"].asDouble();
			swarmsMsg[i].velocity[0] = data["velocityX"].asDouble();
			swarmsMsg[i].velocity[1] = data["velocityY"].asDouble();
			swarmsMsg[i].velocity[2] = data["velocityZ"].asDouble();
			swarmsMsg[i].mode = data["mode"].asString();
			swarmsMsg[i].bridgeMessage = data["bridgeMessage"].asString();
		}
	}
	else {
		std::cout << "convert fails..." << std::endl;
	}
	return;
}

void DroneState::getMsg(const std::string& recvMsg, std::vector<DroneState>& swarmsMsgs, ControlMsg& controlMsg)
{
	swarmsMsgs.clear();
	Json::Reader reader;
	Json::Value recv;
	if (reader.parse(recvMsg.c_str(), recv))
	{
		int swarmNumbers = recv["swarmSize"].asInt();
		Json::Value states = recv["drones"];
		swarmsMsgs.resize(swarmNumbers);
		for (int i = 0; i < swarmNumbers; ++i) {
			Json::Value data = states[i];
			swarmsMsgs[i].DroneNumber = data["droneNumber"].asInt();
			swarmsMsgs[i].position[0] = data["positionX"].asDouble();
			swarmsMsgs[i].position[1] = data["positionY"].asDouble();
			swarmsMsgs[i].position[2] = data["positionZ"].asDouble();
			swarmsMsgs[i].velocity[0] = data["velocityX"].asDouble();
			swarmsMsgs[i].velocity[1] = data["velocityY"].asDouble();
			swarmsMsgs[i].velocity[2] = data["velocityZ"].asDouble();
			swarmsMsgs[i].mode = data["mode"].asString();
			swarmsMsgs[i].bridgeMessage = data["bridgeMessage"].asString();
		}

		Json::Value control = recv["control"];
		if(!control.isNull()){
			controlMsg.droneNumber = control["droneNumber"].asInt();
			controlMsg.position[0] = control["positionX"].asDouble();
			controlMsg.position[1] = control["positionY"].asDouble();
			controlMsg.position[2] = control["positionZ"].asDouble();
			controlMsg.mode = control["mode"].asString();
		}
		else{
			controlMsg.droneNumber = -1;
		}
	}
	else {
		std::cout << "convert fails..." << std::endl;
	}
	return;
}


std::ostream & operator<<(std::ostream & os, const ControlMsg & msg)
{
	os << "droneNumber is:" << msg.droneNumber << std::endl;
	os << "mode is:" << msg.mode << std::endl;
	os << "position is:(" << msg.position[0] << "," << msg.position[1] << "," << msg.position[2] << ")" << std::endl;
	return os;
}

ControlMsg::ControlMsg()
{
	droneNumber = -1;
	position = std::vector<double>(3, 0);
	mode = "LANDING";
}

ControlMsg::ControlMsg(int drone_number)
{
	droneNumber = drone_number;
	position = std::vector<double>(3, 0);
	mode = "LANDING";
}

ControlMsg::ControlMsg(int drone_number, std::string setMode)
{
	droneNumber = drone_number;
	position = std::vector<double>(3, 0);
	mode = setMode;
}